/* Includes ----------------------------------------------------------------- */
#include "integration.h"

#include <math.h>

#include "filter.h"
// 0xFFE1 0xFFE6 0x0006 0xFC7A 0x004A 0x3FD6
/* Private define ----------------------------------------------------------- */
#define _2KP (2.0f * 0.5f)
#define _2KI (2.0f * 0.0f)
#define BETA_IMU (0.9f)
#define BETA_AHRS (0.9f)
#define PI 3.1415926f
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
static float beta = BETA_IMU;
static ComplyFilter_t mpu_comply[3] = {{
                                           .k = 0.01,
                                           .freq = 1000u,
                                       },
                                       {
                                           .k = 0.001,
                                           .freq = 1000u,
                                       },
                                       {
                                           .k = 0.01,
                                           .freq = 1000u,
                                       }};
/* Private function  -------------------------------------------------------- */
static float InvSqrt(float x) {
  float halfx = 0.5f * x;
  float y = x;
  long i = *(long *)&y;
  i = 0x5f3759df - (i >> 1);
  y = *(float *)&i;
  y = y * (1.5f - (halfx * y * y));
  y = y * (1.5f - (halfx * y * y));
  return y;
}
static void EulerLimit(Euler_t *angle) {
  /* yaw :-180-180 */
  if (angle->yaw > 180) {
    angle->yaw -= 360;
  } else if (angle->yaw < -180) {
    angle->yaw += 360;
  }
}

/* Exported functions ------------------------------------------------------- */
int8_t MahonyAHRS_Up(AHRS_t *ahrs, RawFloat_t *raw) {
  if (ahrs == NULL) return -1;
  if (raw == NULL) return -1;

  float ax = raw->acc.x;
  float ay = raw->acc.y;
  float az = raw->acc.z;

  float gx = raw->gyro.x;
  float gy = raw->gyro.y;
  float gz = raw->gyro.z;

  float recip_norm;
  float s0, s1, s2, s3;
  float q_dot1, q_dot2, q_dot3, q_dot4;
  float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2, _8q1, _8q2, q0q0, q1q1, q2q2,
      q3q3;

  /* Rate of change of quaternion from gyroscope */
  q_dot1 =
      0.5f * (-ahrs->quat.q1 * gx - ahrs->quat.q2 * gy - ahrs->quat.q3 * gz);
  q_dot2 =
      0.5f * (ahrs->quat.q0 * gx + ahrs->quat.q2 * gz - ahrs->quat.q3 * gy);
  q_dot3 =
      0.5f * (ahrs->quat.q0 * gy - ahrs->quat.q1 * gz + ahrs->quat.q3 * gx);
  q_dot4 =
      0.5f * (ahrs->quat.q0 * gz + ahrs->quat.q1 * gy - ahrs->quat.q2 * gx);

  /* Compute feedback only if accelerometer measurement valid (avoids NaN in
   * accelerometer normalisation) */
  if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
    /* Normalise accelerometer measurement */
    recip_norm = InvSqrt(ax * ax + ay * ay + az * az);
    ax *= recip_norm;
    ay *= recip_norm;
    az *= recip_norm;

    /* Auxiliary variables to avoid repeated arithmetic */
    _2q0 = 2.0f * ahrs->quat.q0;
    _2q1 = 2.0f * ahrs->quat.q1;
    _2q2 = 2.0f * ahrs->quat.q2;
    _2q3 = 2.0f * ahrs->quat.q3;
    _4q0 = 4.0f * ahrs->quat.q0;
    _4q1 = 4.0f * ahrs->quat.q1;
    _4q2 = 4.0f * ahrs->quat.q2;
    _8q1 = 8.0f * ahrs->quat.q1;
    _8q2 = 8.0f * ahrs->quat.q2;
    q0q0 = ahrs->quat.q0 * ahrs->quat.q0;
    q1q1 = ahrs->quat.q1 * ahrs->quat.q1;
    q2q2 = ahrs->quat.q2 * ahrs->quat.q2;
    q3q3 = ahrs->quat.q3 * ahrs->quat.q3;

    /* Gradient decent algorithm corrective step */
    s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
    s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * ahrs->quat.q1 - _2q0 * ay -
         _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
    s2 = 4.0f * q0q0 * ahrs->quat.q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay -
         _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
    s3 = 4.0f * q1q1 * ahrs->quat.q3 - _2q1 * ax + 4.0f * q2q2 * ahrs->quat.q3 -
         _2q2 * ay;

    /* normalise step magnitude */
    recip_norm = InvSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);

    s0 *= recip_norm;
    s1 *= recip_norm;
    s2 *= recip_norm;
    s3 *= recip_norm;

    /* Apply feedback step */
    q_dot1 -= beta * s0;
    q_dot2 -= beta * s1;
    q_dot3 -= beta * s2;
    q_dot4 -= beta * s3;
  }

  /* Integrate rate of change of quaternion to yield quaternion */
  ahrs->quat.q0 += q_dot1 * 1.0f / ahrs->samplefreq;
  ahrs->quat.q1 += q_dot2 * 1.0f / ahrs->samplefreq;
  ahrs->quat.q2 += q_dot3 * 1.0f / ahrs->samplefreq;
  ahrs->quat.q3 += q_dot4 * 1.0f / ahrs->samplefreq;

  /* Normalise quaternion */
  recip_norm =
      InvSqrt(ahrs->quat.q0 * ahrs->quat.q0 + ahrs->quat.q1 * ahrs->quat.q1 +
              ahrs->quat.q2 * ahrs->quat.q2 + ahrs->quat.q3 * ahrs->quat.q3);
  ahrs->quat.q0 *= recip_norm;
  ahrs->quat.q1 *= recip_norm;
  ahrs->quat.q2 *= recip_norm;
  ahrs->quat.q3 *= recip_norm;

  return 0;
}
/**
 * @brief IMU解算
 *
 * @param ahrs 四元数
 * @param angel 互补滤波角度
 * @param angel_2 卡尔曼滤波角度
 * @param angel_raw 原始角度
 * @param raw 陀螺仪原始数据
 */
void IMU_Prase(AHRS_t *ahrs, Euler_t *angel, Euler_t *angel_2,
               Euler_t *angel_raw, RawFloat_t *raw) {
  static uint8_t filter_init;
  static Filter_Kalman_t filter[3];
  if (!filter_init) {
    Filter_KalmanInit(&filter[0], 0.0005);
    Filter_KalmanInit(&filter[1], 0.0005);
    Filter_KalmanInit(&filter[2], 0.0005);
    filter_init = 1;
  }
  if (filter == NULL)
    ;

  angel_raw->yaw = atan2f(raw->acc.x, raw->acc.z) * 180.f / PI;
  angel_raw->rol = atan2f(raw->acc.x, raw->acc.y) * 180.f / PI;
  angel_raw->pit = atan2f(raw->acc.z, raw->acc.y) * 180.f / PI;

  angel_2->rol = Filter_KalmanApply(angel_raw->rol, raw->gyro.z, &filter[1]);
  angel_2->yaw = Filter_KalmanApply(angel_raw->yaw, raw->gyro.y, &filter[2]);

  angel->pit = Filte_ComplyApply(&mpu_comply[0], angel_raw->pit, raw->gyro.x);
  angel->yaw = Filte_ComplyApply(&mpu_comply[1], angel_2->yaw, raw->gyro.y);
  angel->rol = Filte_ComplyApply(&mpu_comply[2], angel_2->rol, raw->gyro.z);
}
